#include "Frame.h"

using namespace cv::line_descriptor;

namespace Relocation_Slam
{
	
Frame::Frame() {}
Frame::Frame(ORBVocabulary* voc,cv::Mat im,Eigen::Isometry3d pose,double picName) {
    mpORBvocabulary = voc;
    mPose = pose;
    mdPicName = picName;

    image = im;

    ExtractORB(im);
//    ExtractLine(im);

    ComputeBoW();
}
void Frame::ExtractORB(const cv::Mat &im)
{
    detector = cv::ORB::create();
    descriptor = cv::ORB::create();
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( im,mvKeyPoints );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( im, mvKeyPoints, mPointDescriptors );

    mmMapPoints.resize(mvKeyPoints.size());

    mmMapPointsRes.resize(mvKeyPoints.size(),cv::Point3f(0,0,0));

    mbKpTriangle.resize(mvKeyPoints.size(),0);

    mnMappointId.resize(mvKeyPoints.size(),-1);
}

void Frame::ExtractLine(const cv::Mat &im)
{
    bd_line = LSDDetector::createLSDDetector();
    cv::Mat mask = cv::Mat::ones( im.size(), CV_8UC1 );
    std::vector<KeyLine> keylines;
    bd_line->detect( im, keylines, 2, 1, mask );
    bd = BinaryDescriptor::createBinaryDescriptor(  );
    cv::Mat descr;

    ( *bd )( im, mask, keylines, descr, false, false );
    std::vector<KeyLine> lbd_octave;
    for ( int i = 0; i < (int) keylines.size(); i++ )
    {
        if( keylines[i].octave == 0 && keylines[i].lineLength>7)
        {
            lbd_octave.push_back( keylines[i] );
            mLineDescriptors.push_back( descr.row( i ) );
        }
    }
    std::cout<<"mLineDescriptors::"<<mLineDescriptors.rows<<","<<mLineDescriptors.cols<<std::endl;
}

void Frame::CombineMapPoints(){
    for(int i=0;i<mmMapPoints.size();i++){
        float x = 0, y = 0, z = 0;
        if(mmMapPoints[i].size()>1){
            for(int j=0;j<mmMapPoints[i].size();j++){
                x += mmMapPoints[i][j].x;
                y += mmMapPoints[i][j].y;
                z += mmMapPoints[i][j].z;
            }
            mmMapPointsRes[i] = cv::Point3f(x/mmMapPoints[i].size(),y/mmMapPoints[i].size(),z/mmMapPoints[i].size());
        }
    }
}
void Frame::SetFrameId(int frameid){
    mnFrameId = frameid;
}

/**
* @brief Bag of Words Representation
*
* 计算词包mBowVec和mFeatVec，其中mFeatVec记录了属于第i个node（在第4层）的ni个描述子
* @see CreateInitialMapMonocular() TrackReferenceKeyFrame() Relocalization()
*/
void Frame::ComputeBoW()
{
    if(mBowVec.empty())
    {
        std::vector<cv::Mat> vDesc;
        vDesc.reserve(mPointDescriptors.rows);
        for (int j=0;j<mPointDescriptors.rows;j++)
            vDesc.push_back(mPointDescriptors.row(j));
        mpORBvocabulary->transform(vDesc,mBowVec,mFeatVec,4);
//        DBoW2::FeatureVector::const_iterator Fit = mFeatVec.begin();
//        while(Fit!=mFeatVec.end()){
//            std::cout<<Fit->second.size()<<",";
//            Fit++;
//        }

    }
}

} //Relocation_Slam
